
#include "OutPut.h"
#include "MC.h"
#include "OSProc.h"
#include "MutexLock.h"
#include "mmuInit.h"
#include "HAL.h"

//////////////////////////////MACRO////////////////////////////////////////


//////////////////////////////MACRO////////////////////////////////////////

#define AM335X_VECTOR_BASE		(0x80504000)
#define FIR_BASE_ADDR	0x80000000
#define STACK_SIZE	0xA00000
#define HWREG(x)                        (*((volatile unsigned int *)(x)))

#define u32				unsigned int

#define HIGH(n)				(0xFFFFFFFF << n)
#define LOW(n)				(0xFFFFFFFF >> n)

/*****************************************************************************
**                About MMU
*****************************************************************************/
#define PHY_MEMORY_START_ADDR		0x80000000	//512MB
#define PHY_HARD_START_ADDR		0x44000000	//-0x56FFFFFF
#define PHY_OCMC_START_ADDR		0x402F0400	//1MB
#define FIR_MMU_PAGE_TAB		0x80000000	//4KB
#define SEC_MMU_PAGE_TAB		0x80004000	//4MB
#define VMR_BASE			0x80404000	// - 0x80504000 1MB
//#define	PVR_BASE			0x80504000	// - 0x80584000	512KB
//#define VEC_TAB_BASE			0x80584000	// - 0x80504400	1KB
#define VEC_TAB_BASE			0x80504400	// - 0x80504400	1KB
#define ORG_VEC_TAB_BASE		0x4030FC00	//14*4 byte
#define ORG_VEC_SIZE			14

#define		VMA(n)			(n)	//Simple reflection

#define		HVMA(n)			(n)		//not reflect first

#define MMU_FULL_ACCESS     (0xFF << 4)   /* 访问权限AP */
#define MMU_DOMAIN          (0 << 5)    /* 属于哪个域 Domain*/
#define MMU_SPECIAL         (1 << 4)    /* 必须是1 */
#define MMU_CACHEABLE       (1 << 3)    /* cacheable C位*/
#define MMU_BUFFERABLE      (1 << 2)    /* bufferable B位*/
#define MMU_ROUGH_PAGE		(1)         /* 表示这是一级页描述符 */
#define MMU_SMALL_PAGE		(2)        //二级页表描述符
#define MMU_SECDESC         (MMU_DOMAIN | MMU_ROUGH_PAGE)
#define MMU_SMALLPAGE_SECDESC ( MMU_FULL_ACCESS | MMU_CACHEABLE | MMU_BUFFERABLE | MMU_SMALL_PAGE )
//#define MMU_SECDESC_WB      (MMU_FULL_ACCESS | MMU_DOMAIN | MMU_SPECIAL | MMU_CACHEABLE | MMU_BUFFERABLE | MMU_SMALL_PAGE)
#define MMU_PAGE_SIZE    (0x00001000)        /*每个页描述符对应4KB大小空间*/

#define MMU_LEV_1	20
#define MMU_LEV_2	12




//////////////////////////////Var///////////////////////////////////////////


volatile extern unsigned int _heap;
volatile extern unsigned int _stack;

//volatile unsigned char IFD0Memory[20*1024];
//volatile unsigned char IFD1Memory[20*1024];

extern volatile ring* run;
extern volatile ring* sleep;

volatile ring buff;
volatile _pcb init;
volatile _tcb initthr;

static unsigned int const vecTbl[14]=
{
    0xE59FF018,    /* Opcode for loading PC with the contents of [PC + 0x18] */
    0xE59FF018,    /* Opcode for loading PC with the contents of [PC + 0x18] */
    0xE59FF018,    /* Opcode for loading PC with the contents of [PC + 0x18] */
    0xE59FF018,    /* Opcode for loading PC with the contents of [PC + 0x18] */
    0xE59FF014,    /* Opcode for loading PC with the contents of [PC + 0x14] */
    0xE24FF008,    /* Opcode for loading PC with (PC - 8) (eq. to while(1)) */
    0xE59FF010,    /* Opcode for loading PC with the contents of [PC + 0x10] */
    0xE59FF010,    /* Opcode for loading PC with the contents of [PC + 0x10] */
    (unsigned int)Entry,
    (unsigned int)UndefInstHandler,
    (unsigned int)SVC_Handler,
    (unsigned int)AbortHandler,
    (unsigned int)IRQHandler,
    (unsigned int)FIQHandler
};


//////////////////////////////Function//////////////////////////////////////

void MMUInitialsss();

volatile _klock kernellock;
volatile _mch kmemory;

static void CopyVectorTable(void)
{
    unsigned int *dest = (unsigned int *)AM335X_VECTOR_BASE;
    unsigned int *src =  (unsigned int *)vecTbl;
    unsigned int count;
  
    CP15VectorBaseAddrSet(AM335X_VECTOR_BASE);

    for(count = 0; count < sizeof(vecTbl)/sizeof(vecTbl[0]); count++)
    {
        dest[count] = src[count];
    }


}

unsigned int start_boot()
{
	CopyVectorTable();
	main();
//Power off
		while(1);
}

int main()
{

	BlPlatformMMCSDSetup();
	 MMUInitialsss();
	INVALIDATE_ALL_TLB();
	HALInit();
		unsigned int __heap = (unsigned int)&_heap;
		unsigned int __stack = (unsigned int)&_stack;
		unsigned int HEAP_SIZE = __stack - STACK_SIZE  - __heap;

	

	QueueInit(&(kernellock.waiting),KERNEL_WAITING,sizeof(ring*),KERNEL_WAITING_THREAD);
	MemoryControlInit((_mch*)&kmemory,(unsigned long*)__heap,(unsigned long)HEAP_SIZE);




	kernellock.lnow = NULL;


	unsigned int initthr_argv[5];
	initthr_argv[0] = FIR_MMU_PAGE_TAB;
	init.flPage = (unsigned long*)FIR_BASE_ADDR;
	initthr.pPro = &init;
	initthr.init_argv = initthr_argv;


	
	buff.content = &initthr;
	run = &buff;
	run -> forw = (ring*)run;
	run -> next = (ring*)run;
	sleep = Kmalloc((_mch*)&kmemory,sizeof(ring));

	sleep->forw = sleep;
	sleep->next = sleep;
	sleep->content = NULL;

	IFDInit();
//	ExcuteFile("test");



	while(0);
	return 0;
}

void MMUInitialsss()
{

		u32  phyAddr;
		//0x00000000~... need to be zero
		//Kernel from 0x80000000 to 0xF00000000 per  4KB
//		MMUFLDInit(0x80000000);
		for(phyAddr = 0; phyAddr < PHY_MEMORY_START_ADDR ;phyAddr += 0x00100000)
		{
			HWREG(FIR_MMU_PAGE_TAB + (( (VMA(phyAddr) >> MMU_LEV_1)) << 2) )
				 = 0 ;
		}
		for(phyAddr = PHY_MEMORY_START_ADDR;phyAddr <= 0x8FFFFFFF ; phyAddr += 0x00100000)
		{
			HWREG(FIR_MMU_PAGE_TAB + (( (VMA(phyAddr) >> MMU_LEV_1)) << 2) )
				 = 0 ;
			HWREG(FIR_MMU_PAGE_TAB + (( (VMA(phyAddr) >> MMU_LEV_1)) << 2) )
				 = ((SEC_MMU_PAGE_TAB + ((VMA(phyAddr) & 0x0FF00000) >> 6 )) & HIGH(10)) | FL_NORMAL  ;

			u32 secPhyAddr;

			for(secPhyAddr = phyAddr; secPhyAddr < phyAddr + 0x000FFFFF ; secPhyAddr += MMU_PAGE_SIZE)
			{
/*				HWREG	 ((SEC_MMU_PAGE_TAB +
					 (((VMA(phyAddr) & 0x0FF00000) >> 6 ) & HIGH(10))) +
					 (((VMA(secPhyAddr) & 0x000FF000) >> MMU_LEV_2) << 2))
					= 0;
				HWREG	 ((SEC_MMU_PAGE_TAB +
					 (((VMA(phyAddr) & 0x0FF00000) >> 6 ) & HIGH(10))) +
					 (((VMA(secPhyAddr) & 0x000FF000) >> MMU_LEV_2) << 2))
					= (secPhyAddr & HIGH(12)) | SL_STRONG_NORMAL;*/
				MMURemap4KB(FIR_MMU_PAGE_TAB,VMA(secPhyAddr),secPhyAddr);
			}
		}






		//Hard device from 0x44000000 to HVMA(i haven't decided) per 1MB
			for(phyAddr = PHY_HARD_START_ADDR;phyAddr <  0x56FFFFFF ; phyAddr += 0x00100000)
			{
				HWREG(FIR_MMU_PAGE_TAB + (( (HVMA(phyAddr) >> MMU_LEV_1)) << 2) )
						= (phyAddr & HIGH(20)) | 0x80C16  ;
			}
		//OCMC RAM don't remap and it's not very good 'cause i cover 1MB but only 64 KB should be covered
		HWREG(FIR_MMU_PAGE_TAB + ((PHY_OCMC_START_ADDR >> MMU_LEV_1) << 2))
				= (PHY_OCMC_START_ADDR & HIGH(20)) | 0x82C06  ;

		//Page init success;
		//There should be no problem
		u32 offset;
		for(offset = 0;offset < 0x00100000 ; offset+=4) //不允许非对齐访问
		{
/*			if(offset >= 0x00010000)
			{
				HWREG(VMR_BASE + offset) = 0x01010101;
			}
			else*/
			{
				HWREG(VMR_BASE + offset) = 0;
			}

		}




/*		for(offset = 0 ;offset < 0x00080000 ; offset += sizeof(u32))
		{
			if(offset < 0x00040000)
			{
				HWREG(PVR_BASE + offset) =
					VMA(PHY_MEMORY_START_ADDR) +
					offset * (MMU_PAGE_SIZE / sizeof(u32));
			}
			else
			{
				HWREG(PVR_BASE + offset) = 0;
			}
		}*/


	__asm__ __volatile__(

	"mov r0, #0  ;"
	"mcr p15, 0, r0, c7, c5, 1 ;"
	"mcr p15, 0, r0, c7, c6, 1 ;"
	"mcr p15, 0, r0, c7, c10, 4;"
	"mcr p15, #0, r0, c8, c7, #0;"//	@invaliadate I,D TLB
//	"ldr r0, =0x55555555	;	"//	@move (not #0)
	"movw r0, #0x5555;"
	"movt r0, #0x5555;"
//			"movw r0, #0xFFFF;"
//			"movt r0, #0xFFFF;"
	"mcr p15, #0, r0, c3, c0, #0;	"//@all ACCESS
	"mrc p15, #0, r0, c1, c0, #0;	"//@get the control register
	"bic r0, r0, #0x20000000;"
	"bic r0, r0, #0x10000000;"
	"bic r0, r0, #0x0002	;"	//	@open the I C A M	Close A
	"bic r0, r0, #0x0001	;"	//	@open the I C A M
	"mcr p15, #0, r0, c1, c0, #0;"	//@set the control register this is where the problems are
	"mov r0, #0;"
    	"mcr p15, #0, r0, c2, c0, #2;"
	"ldr r1, =0x80000000;	"	//@get the Base address
	"mcr p15, #0, r1, c2, c0, #0;"	//@set the Base address
	"mrc p15, 0, r0, c1, c0, 0;"	//@get the control register
	"bic r0, r0, #0x3000;"
	"bic r0, r0, #0x0007;"
	"orr r0, r0, #0x0001;"		//@open the I C M Close A
	"mcr p15, 0, r0, c1, c0, 0;"	//@set the control register this is where the problems are


:
:
:

);

					CacheDisable(0x03);  //written in /system/armv7a/cache.c
						//	/system/armv7a/gcc/cp5.s

}
